/*
 * @Description:
 * @Version:
 * @Author: Troubadour
 * @Date: 2022-08-31 21:59:42
 * @LastEditors: Troubadour
 * @LastEditTime: 2022-10-07 20:59:57
 */

#ifndef  __ROBOTSERVOFW_H__
#define  __ROBOTSERVOFW_H__

#include <Arduino.h>

// typedef unsigned char  uint8_t;
// typedef unsigned short uint16_t;
// typedef unsigned int   uint32_t;
// typedef char  int8_t;
// typedef short int16_t;
// typedef int   int32_t;

#define LOBOT_SERVO_FRAME_HEADER         0x55
#define LOBOT_SERVO_MOVE_TIME_WRITE      1
#define LOBOT_SERVO_MOVE_TIME_READ       2
#define LOBOT_SERVO_MOVE_TIME_WAIT_WRITE 7
#define LOBOT_SERVO_MOVE_TIME_WAIT_READ  8
#define LOBOT_SERVO_MOVE_START           11
#define LOBOT_SERVO_MOVE_STOP            12
#define LOBOT_SERVO_ID_WRITE             13
#define LOBOT_SERVO_ID_READ              14
#define LOBOT_SERVO_ANGLE_OFFSET_ADJUST  17
#define LOBOT_SERVO_ANGLE_OFFSET_WRITE   18
#define LOBOT_SERVO_ANGLE_OFFSET_READ    19
#define LOBOT_SERVO_ANGLE_LIMIT_WRITE    20
#define LOBOT_SERVO_ANGLE_LIMIT_READ     21
#define LOBOT_SERVO_VIN_LIMIT_WRITE      22
#define LOBOT_SERVO_VIN_LIMIT_READ       23
#define LOBOT_SERVO_TEMP_MAX_LIMIT_WRITE 24
#define LOBOT_SERVO_TEMP_MAX_LIMIT_READ  25
#define LOBOT_SERVO_TEMP_READ            26
#define LOBOT_SERVO_VIN_READ             27
#define LOBOT_SERVO_POS_READ             28
#define LOBOT_SERVO_OR_MOTOR_MODE_WRITE  29
#define LOBOT_SERVO_OR_MOTOR_MODE_READ   30
#define LOBOT_SERVO_LOAD_OR_UNLOAD_WRITE 31
#define LOBOT_SERVO_LOAD_OR_UNLOAD_READ  32
#define LOBOT_SERVO_LED_CTRL_WRITE       33
#define LOBOT_SERVO_LED_CTRL_READ        34
#define LOBOT_SERVO_LED_ERROR_WRITE      35
#define LOBOT_SERVO_LED_ERROR_READ       36

/* 舵机指令缓冲区大小 */
#define CMD_Size            20
/* 每行前6个值按顺序对应舵机ID，第7个为舵机运行时间 */
#define Array_size          7

//宏函数 获得A的低八位
#define GET_LOW_BYTE(A) (uint8_t)((A))
//宏函数 获得A的高八位
#define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
//宏函数 以A为高八位 B为低八位 合并为16位整形
#define BYTE_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B))



class RobotServoFw
{
private:
    uint8_t LobotCheckSum(uint8_t (&buf)[CMD_Size]);
    void LobotSerialServoMove(uint8_t id, int16_t position, uint16_t time);

public:
    RobotServoFw();
    RobotServoFw(HardwareSerial &Handle);
    ~RobotServoFw();

    void Robot_ActionArray(const uint16_t ActionArray[][Array_size], uint8_t size);
    void Robot_Running(void);
    void Robot_Back(void);
    void Robot_LeftHanded(void);
    void Robot_RightHanded(void);
    void Robot_Stand(void);
    void Robot_ForwardRoll(void);
    void Robot_BackwardRoll(void);
    void Robot_Crouch(void);

    HardwareSerial *SerialX;
};


#endif  /* __ROBOTSERVOFW_H__ */
